#!/usr/bin/env python3
# -*- coding: utf-8 -*-
from __future__ import absolute_import
from __future__ import print_function
import rospy
import math
from nav_msgs.msg import Odometry
from std_msgs.msg import Int32

def leader_odom_cb(data):
    global leader_odom
    leader_odom = data

def distance(x1, y1, x2, y2):
    return math.sqrt(math.pow(x1 - x2, 2) + math.pow(y1 - y2, 2))

if __name__ == '__main__':
    rospy.init_node('whether_to_target_node')
    rospy.loginfo("Whether to reach the destination!!")

    cwm_flag = Int32()

    # 目标点
    target1_x = 5.0
    target1_y = 5.0
    target2_x = 5.0
    target2_y = 0.0
    target3_x = 5.0
    target3_y = -5.0

    leader_odom = Odometry()

    leader_odom_sub = rospy.Subscriber('/tb3_0/odom', Odometry, leader_odom_cb)
    pub_flag = rospy.Publisher('/cwm_flag', Int32, queue_size=1)

    # print(leader_odom)
    rate = rospy.Rate(10) # 10hz
    while not rospy.is_shutdown():
        # print(leader_odom)
        leader_x = leader_odom.pose.pose.position.x
        leader_y = leader_odom.pose.pose.position.y

        distance_to_A = distance(leader_x, leader_y, target1_x, target1_y)
        distance_to_B = distance(leader_x, leader_y, target2_x, target2_y)
        distance_to_C = distance(leader_x, leader_y, target3_x, target3_y)
        # print("distance_to_A: %lf" %distance_to_A)
        # print("distance_to_B: %lf" %distance_to_B)
        # print("distance_to_C: %lf" %distance_to_C)

        if (distance_to_A > 0.4 and distance_to_B > 0.4 and distance_to_C > 0.4):
            cwm_flag.data = 0
            pub_flag.publish(cwm_flag)
        elif (distance_to_A <= 0.4):
            print("Target to A !!!!!!!")
            cwm_flag.data = 1
            pub_flag.publish(cwm_flag)
        elif (distance_to_B <= 0.4):
            print("Target to B !!!!!!!")
            cwm_flag.data = 2
            pub_flag.publish(cwm_flag)
        elif (distance_to_C <= 0.4):
            print("Target to C !!!!!!!")
            cwm_flag.data = 3
            pub_flag.publish(cwm_flag)

        rate.sleep()
